#include <iostream>
#include "SMS_STS.h"
#include "Public.h"
#include <math.h>

#include "IK_xbk.h"
#include "myEigen.h"

#define PI 3.14159265358979
using namespace std;

SMS_STS sm_st;
TTimer tmr;

u8 ID[6] = {1, 2, 3, 4, 5, 6};
s16 angle_pos;
s16 Position[6];
s16 Init_Position[6] = {2047, 2047, 2047, 2047, 2047, 2047};
u16 angle_speed = 200;
u8 angle_acc = 200;
u16 Speed[6] = {angle_speed, angle_speed, angle_speed, angle_speed, angle_speed, angle_speed};
u8 ACC[6] = {angle_acc, angle_acc, angle_acc, angle_acc, angle_acc, angle_acc};
double direction_flag[6] = {-1, 1, -1, 1, -1, -1};
s16 offset_angle_zero[6] = {2047, 3070, 2047, 3070, 3070, 2047};
//max limit
s16 joint_down_limit[6] = {30,  1024,   512,    512,    1024, 1024};
s16 joint_up_limit[6] = {4066,  3072,   3584,   3584,   3072,3072};

// s16 joint_down_limit[6] = {30,  1536,   512,    512,    1024, 1024};
// s16 joint_up_limit[6] = {4066,  2560,   3584,   3584,   3072,3072};

// float k = 0.000001;
float k = 0.5;
int Pos;
int feedback_pos = 2047;
double feedback_pos_rad[6];

void update_read_angle();
void set_pos_encode(Mat61 set_pos_float);
Mat61 nearest_solution(Mat44 T);

int main(int argc, char **argv)
{
    if (!sm_st.begin(1000000, "/dev/ttyUSB0"))
    {
        std::cout << "Failed to init sts motor!" << std::endl;
        return 0;
    }


    int Load;
    s16 test_Position[6] = {2047, 2047, 2047, 2047, 2047, 2047};
   

    
    for (int j = 0; j < 6; j++) //first init position
    {
        Position[j] = Init_Position[j];
    }
    sm_st.SyncWritePosEx(ID, 6, Position, Speed, ACC);

    sleep(10);

    Mat61 set_pos;
    // set_pos << -1214,0.0432,1.3646,1.9285, 0, 0;
    set_pos << 0,-0.5,-0.5,0.5, 0, 0;
    set_pos_encode(set_pos);
    sm_st.SyncWritePosEx(ID, 6, Position, Speed, ACC);



    /*

    //ik test
    Mat44 T;
    Mat61 theta;

    double y_det = -0.35;

    T << -0.600743, 0.491296, 0.630664, -0.1,
        -0.0482734, 0.765147, -0.642043, y_det,
        -0.797984, -0.416147, -0.43594, 0.4,
        0, 0, 0, 1;

    // theta << 1, 0, -1, -1, -0.5, 0;
    // cout << kinematics(theta) << endl;
    // // T <<
    theta = nearest_solution(T);
    set_pos_encode(theta);
    sm_st.SyncWritePosEx(ID, 6, Position, Speed, ACC);
    sleep(2);

    tmr.Clear();
    while (1)
    {
        if (tmr.GetValue() < 1)
        {
            T << -0.600743, 0.491296, 0.630664, -0.1 + 0.2 * tmr.GetValue(),
                -0.0482734, 0.765147, -0.642043, y_det,
                -0.797984, -0.416147, -0.43594, 0.4,
                0, 0, 0, 1;
            theta = nearest_solution(T);
            set_pos_encode(theta);
            sm_st.SyncWritePosEx(ID, 6, Position, Speed, ACC);
        }else if(tmr.GetValue() < 2)
        {
            T << -0.600743, 0.491296, 0.630664, 0.1 ,
                -0.0482734, 0.765147, -0.642043, y_det,
                -0.797984, -0.416147, -0.43594, 0.4 -  0.1 * (tmr.GetValue()- 1),
                0, 0, 0, 1;
            theta = nearest_solution(T);
            set_pos_encode(theta);
            sm_st.SyncWritePosEx(ID, 6, Position, Speed, ACC);
        }else if(tmr.GetValue() < 3)
        {
            T << -0.600743, 0.491296, 0.630664, 0.1 -  0.2 * (tmr.GetValue()- 2),
                -0.0482734, 0.765147, -0.642043, y_det,
                -0.797984, -0.416147, -0.43594, 0.3,
                0, 0, 0, 1;
            theta = nearest_solution(T);
            set_pos_encode(theta);
            sm_st.SyncWritePosEx(ID, 6, Position, Speed, ACC);
        }else if(tmr.GetValue() < 4)
        {
            T << -0.600743, 0.491296, 0.630664, -0.1 ,
                -0.0482734, 0.765147, -0.642043, y_det,
                -0.797984, -0.416147, -0.43594, 0.3 +  0.1 * (tmr.GetValue()- 3),
                0, 0, 0, 1;
            theta = nearest_solution(T);
            set_pos_encode(theta);
            sm_st.SyncWritePosEx(ID, 6, Position, Speed, ACC);
        }else{
            tmr.Clear();
        }
        
        usleep(10000);
    }
    */
    sm_st.end();

    return 1;
}

void update_read_angle()
{
    for (int i = 0; i < 6; i++)
    {
        feedback_pos_rad[i] = (double)((sm_st.ReadPos(i + 1) - offset_angle_zero[i]) * direction_flag[i] * PI / 2048.0);
        // cout << "j:" << i + 1 << " " << sm_st.ReadPos(i + 1) << "\t";
    }
    // cout << endl;
}

void set_pos_encode(Mat61 set_pos_float)
{
    for (int i = 0; i < 6; i++)
    {
        while(set_pos_float[i] < -PI) set_pos_float[i] = 2*PI + set_pos_float[i];
        while(set_pos_float[i] > PI) set_pos_float[i] = -2*PI + set_pos_float[i];

        Position[i] = (s16)(set_pos_float[i] * direction_flag[i] / PI * 2048.0 + offset_angle_zero[i]);
        // cout<<Position[i]<<" ";
        if(Position[i] > joint_up_limit[i])
        {
            cout<<"joint "<<i+1<<" is "<<Position[i]<<", so big, set to "<< joint_up_limit[i]<<"!"<<endl;
            Position[i] = joint_up_limit[i];
        }
        if(Position[i] < joint_down_limit[i])
        {
            cout<<"joint "<<i+1<<" is "<<Position[i]<<", so small, set to "<< joint_down_limit[i]<<"!"<<endl;
            Position[i] = joint_down_limit[i];
        }

    }
    // cout<<endl;
}

Mat61 nearest_solution(Mat44 T)
{
    Mat86 Solution86;
    Solution86 = Inverse_kinematics(T);
    // cout<<"Solution:"<<endl;
    // cout<<Solution86<<endl;

    double q_nearest[6];
    int index = 0;

    double distance;
    double length[8] = {0};

    update_read_angle(); //update feedback_pos_rad

    for (int i = 0; i < 8; i++)
    {
        for (int j = 0; j < 6; j++)
        {
            distance = Solution86(i, j) - feedback_pos_rad[j];
            distance = fabs(PI - fabs(fabs(distance) - PI));
            length[i] += distance;
        }
        // if(Solution86(i, 2) > 0) length[i] += 1;

        if (length[i] < length[index])
            index = i;
    }
    Mat61 ret;
    for (int i = 0; i < 6; i++)
    {
        ret[i] = Solution86(index, i);
    }

    return ret;
}
